#include "Ode45.h"
#include "MultibodySystem.h"
#include "SystemData.h"
#include "State.h"

using namespace RSIM;

void Ode45::attemptSingleStep(void(*calcYDot)(const double& ,const Vector&,Vector&), // function f(t,y) = dy/dt
                                        const double& t0, 	  // given: t
                                        const Vector& Y0, 	  // given: Y(t)
                                        const Vector& Y0Dot,	  // given: yDot(t)
                                        const double& h, 	  // given: deltaT
                                        Vector& Y_deltaT, 	  // calculate:Y(t+deltaT)
                                        Vector& Y1Dot,	  	  // calculate:YDot(t+deltaT)
                                        double& error		  // calculate: estimate of local error in this step.
                        ){	
        // Embeded Runge-Kutta-Fehlberg 4(5)
        // Table 5.1 of \cite{hairer1993}

        const double& C21   =  1.0/5.0;
        const double& C22   =  1.0/5.0;

        const double& C31   =  3.0/10.0;
        const double& C32   =  3.0/40.0;
        const double& C33   =  9.0/40.0;

        const double& C41   =  4.0/5.0;	
        const double& C42   =  44.0/45.0;
        const double& C43   = -56.0/15.0;
        const double& C44   =  32.0/9.0;

        const double& C51   = 8.0/9.0; 
        const double& C52   = 19372.0/6561.0;
        const double& C53   = -25360.0/2187.0;
        const double& C54   = 64448.0/6561.0;
        const double& C55   = -212.0/729.0;

        const double& C61   = 1.0;
        const double& C62   = 9017.0/3168.0;
        const double& C63   = -355.0/33.0;
        const double& C64   = 46732.0/5247.0;
        const double& C65   = 49.0/176.0;
        const double& C66   = -5103.0/18656.0;

        const double& C71   = 1.0;
        const double& C72   = 35.0/384.0;
        const double& C73   = 0.0;
        const double& C74   = 500.0/1113.0;
        const double& C75   = 125.0/192.0;
        const double& C76   = -2187.0/6784.0;
        const double& C77   = 11.0/84.0;

        const double& CE1   = 71.0/57600.0;
        const double& CE2   = 0.0;
        const double& CE3   = -71.0/16695.0;
        const double& CE4   = 71.0/1920.0;
        const double& CE5   = -17253.0/339200.0;
        const double& CE6   = 22.0/525.0;
        const double& CE7   = -1.0/40.0;
        
        // new labels for intermediate terms
        const Vector& k0 = Y0Dot;
        Vector& k1 = k[0];
        Vector& k2 = k[1];
        Vector& k3 = k[2];
        Vector& k4 = k[3];
        Vector& k5 = k[4];
        Vector& k6 = Y1Dot;
        
        // sFlag	
        calcYDot(t0+C21*h, Y0+(h*C22)*k0, k1);
        calcYDot(t0+C31*h, Y0+(h*C32)*k0+(h*C33)*k1, k2);	
        calcYDot(t0+C41*h, Y0+(h*C42)*k0+(h*C43)*k1+(h*C44)*k2, k3);
        calcYDot(t0+C51*h, Y0+(h*C52)*k0+(h*C53)*k1+(h*C54)*k2+(h*C55)*k3, k4);
        calcYDot(t0+C61*h, Y0+(h*C62)*k0+(h*C63)*k1+(h*C64)*k2+(h*C65)*k3+(h*C66)*k4, k5);
        
        Y_deltaT = Y0+(h*C72)*k0+(h*C73)*k1+(h*C74)*k2+(h*C75)*k3+(h*C76)*k4+(h*C77)*k5;
        calcYDot(t0+C71*h, Y_deltaT, k6);
        
        // sFlag
        this->estimateLocalStepError(Y0,
                                Y_deltaT,
                                CE1*k0+CE2*k1+CE3*k2+CE4*k3+CE5*k4+CE6*k5+CE7*k6, 
                                h,
                                error);
}

void Ode45::attemptSingleStep(  const double& t0,       // t0
                                const Vector& Y0,               // Y(t0)
                                const Vector& Y0Dot,            // dy/dt evaluated at t0
                                const double& h,           // timeStep
                                SystemData& Data,
                                Vector& Y1,                     // Y(t+deltaT)
                                Vector& Y1Dot,                  // dy/dt evaluated at t+deltaT
                                double& error                   // local error in this time-step
                               ){
        // Embeded Runge-Kutta-Fehlberg 4(5)
        // Table 5.1 of \cite{hairer1993}
        
        const double& C21   =  1.0/5.0;
        const double& C22   =  1.0/5.0;
        
        const double& C31   =  3.0/10.0;
        const double& C32   =  3.0/40.0;
        const double& C33   =  9.0/40.0;
        
        const double& C41   =  4.0/5.0; 
        const double& C42   =  44.0/45.0;
        const double& C43   = -56.0/15.0;
        const double& C44   =  32.0/9.0;
        
        const double& C51   = 8.0/9.0; 
        const double& C52   = 19372.0/6561.0;
        const double& C53   = -25360.0/2187.0;
        const double& C54   = 64448.0/6561.0;
        const double& C55   = -212.0/729.0;
        
        const double& C61   = 1.0;
        const double& C62   = 9017.0/3168.0;
        const double& C63   = -355.0/33.0;
        const double& C64   = 46732.0/5247.0;
        const double& C65   = 49.0/176.0;
        const double& C66   = -5103.0/18656.0;
        
        const double& C71   = 1.0;
        const double& C72   = 35.0/384.0;
        const double& C73   = 0.0;
        const double& C74   = 500.0/1113.0;
        const double& C75   = 125.0/192.0;
        const double& C76   = -2187.0/6784.0;
        const double& C77   = 11.0/84.0;
        
        const double& CE1   = 71.0/57600.0;
        const double& CE2   = 0.0;
        const double& CE3   = -71.0/16695.0;
        const double& CE4   = 71.0/1920.0;
        const double& CE5   = -17253.0/339200.0;
        const double& CE6   = 22.0/525.0;
        const double& CE7   = -1.0/40.0;
        
        // new labels for intermediate terms
        const Vector& k0 = Y0Dot;
        Vector& k1 = k[0];
        Vector& k2 = k[1];
        Vector& k3 = k[2];
        Vector& k4 = k[3];
        Vector& k5 = k[4];
        Vector& k6 = Y1Dot;
        
        Vector y_tmp(numY_);
        
        
        // k1 = dy/dt evaluated at time (t0+C21*h) for the state vector (Y0+(h*C22)*k0)
        Data.Time() = t0+C21*h;
        y_tmp = Y0+(h*C22)*k0;
        Data.setY(y_tmp);
        Data.calcSystemAcceleration();
        Data.getYDot(k1);

        
        // k2 = dy/dt evaluated at time (t0+C31*h) for the state vector (Y0+(h*C32)*k0+(h*C33)*k1)
        Data.Time() = t0+C31*h;
        y_tmp = Y0+(h*C32)*k0+(h*C33)*k1;       
        Data.setY(y_tmp);
        Data.calcSystemAcceleration();
        Data.getYDot(k2);
        
        // k3 = dy/dt evaluated at time (t0+C41*h) for the state vector (Y0+(h*C42)*k0+(h*C43)*k1+(h*C44)*k2)
        Data.Time() = t0+C41*h;        
        y_tmp = Y0+(h*C42)*k0+(h*C43)*k1+(h*C44)*k2;    
        Data.setY(y_tmp);
        Data.calcSystemAcceleration();
        Data.getYDot(k3);
        
        // k4 = dy/dt evaluated at time (t0+C51*h) for the state vector (Y0+(h*C52)*k0+(h*C53)*k1+(h*C54)*k2+(h*C55)*k3)
        Data.Time() = t0+C51*h;        
        y_tmp = Y0+(h*C52)*k0+(h*C53)*k1+(h*C54)*k2+(h*C55)*k3; 
        Data.setY(y_tmp);
        Data.calcSystemAcceleration();
        Data.getYDot(k4);
        
        // k5 = dy/dt evaluated at time (t0+C61*h) for the state vector (Y0+(h*C62)*k0+(h*C63)*k1+(h*C64)*k2+(h*C65)*k3+(h*C66)*k4)
        Data.Time() = t0+C61*h;        
        y_tmp = Y0+(h*C62)*k0+(h*C63)*k1+(h*C64)*k2+(h*C65)*k3+(h*C66)*k4;      
        Data.setY(y_tmp);
        Data.calcSystemAcceleration();
        Data.getYDot(k5);
        
        // k6 = dy/dt evaluated at time (t0+C71*h) for the state vector Y1
        Y1 = Y0+(h*C72)*k0+(h*C73)*k1+(h*C74)*k2+(h*C75)*k3+(h*C76)*k4+(h*C77)*k5;
        Data.Time() = t0+C71*h;                
        Data.setY(Y1);
        Data.calcSystemAcceleration();
        Data.getYDot(k6);
        
        this->estimateLocalStepError(   Y0,
                                        Y1,
                                        CE1*k0+CE2*k1+CE3*k2+CE4*k3+CE5*k4+CE6*k5+CE7*k6, 
                                        h,
                                        error);
}

void Ode45::attemptSingleStep(	const double& t0, 	// t0
                                const Vector& Y0, 		// Y(t0)
                                const Vector& Y0Dot,		// dy/dt evaluated at t0
                                const double& h,		// timeStep
                                MultibodySystem& MBS,		
                                State& state,
                                Vector& Y1,			// Y(t+deltaT)
                                Vector& Y1Dot,			// dy/dt evaluated at t+deltaT
                                double& error 			// local error in this time-step
                                ){
        
        // Embeded Runge-Kutta-Fehlberg 4(5)
        // Table 5.1 of \cite{hairer1993}

        const double& C21   =  1.0/5.0;
        const double& C22   =  1.0/5.0;

        const double& C31   =  3.0/10.0;
        const double& C32   =  3.0/40.0;
        const double& C33   =  9.0/40.0;

        const double& C41   =  4.0/5.0;	
        const double& C42   =  44.0/45.0;
        const double& C43   = -56.0/15.0;
        const double& C44   =  32.0/9.0;

        const double& C51   = 8.0/9.0; 
        const double& C52   = 19372.0/6561.0;
        const double& C53   = -25360.0/2187.0;
        const double& C54   = 64448.0/6561.0;
        const double& C55   = -212.0/729.0;

        const double& C61   = 1.0;
        const double& C62   = 9017.0/3168.0;
        const double& C63   = -355.0/33.0;
        const double& C64   = 46732.0/5247.0;
        const double& C65   = 49.0/176.0;
        const double& C66   = -5103.0/18656.0;

        const double& C71   = 1.0;
        const double& C72   = 35.0/384.0;
        const double& C73   = 0.0;
        const double& C74   = 500.0/1113.0;
        const double& C75   = 125.0/192.0;
        const double& C76   = -2187.0/6784.0;
        const double& C77   = 11.0/84.0;

        const double& CE1   = 71.0/57600.0;
        const double& CE2   = 0.0;
        const double& CE3   = -71.0/16695.0;
        const double& CE4   = 71.0/1920.0;
        const double& CE5   = -17253.0/339200.0;
        const double& CE6   = 22.0/525.0;
        const double& CE7   = -1.0/40.0;
        
        // new labels for intermediate terms
        const Vector& k0 = Y0Dot;
        Vector& k1 = k[0];
        Vector& k2 = k[1];
        Vector& k3 = k[2];
        Vector& k4 = k[3];
        Vector& k5 = k[4];
        Vector& k6 = Y1Dot;
        
        Vector y_tmp(numY_);
        
        // k1 = dy/dt evaluated at time (t0+C21*h) for the state vector (Y0+(h*C22)*k0)
        state.setTime(t0+C21*h);
        y_tmp = Y0+(h*C22)*k0;	
        state.setY(y_tmp.Ptr());
        MBS.calcSystemAcceleration(state);
        state.getYDot(k1.wPtr());
                
        // k2 = dy/dt evaluated at time (t0+C31*h) for the state vector (Y0+(h*C32)*k0+(h*C33)*k1)
        state.setTime(t0+C31*h);
        y_tmp = Y0+(h*C32)*k0+(h*C33)*k1;	
        state.setY(y_tmp.Ptr());
        MBS.calcSystemAcceleration(state);
        state.getYDot(k2.wPtr());
                
        // k3 = dy/dt evaluated at time (t0+C41*h) for the state vector (Y0+(h*C42)*k0+(h*C43)*k1+(h*C44)*k2)
        state.setTime(t0+C41*h);	
        y_tmp = Y0+(h*C42)*k0+(h*C43)*k1+(h*C44)*k2;	
        state.setY(y_tmp.Ptr());
        MBS.calcSystemAcceleration(state);
        state.getYDot(k3.wPtr());
        
        // k4 = dy/dt evaluated at time (t0+C51*h) for the state vector (Y0+(h*C52)*k0+(h*C53)*k1+(h*C54)*k2+(h*C55)*k3)
        state.setTime(t0+C51*h);	
        y_tmp = Y0+(h*C52)*k0+(h*C53)*k1+(h*C54)*k2+(h*C55)*k3;	
        state.setY(y_tmp.Ptr());
        MBS.calcSystemAcceleration(state);
        state.getYDot(k4.wPtr());

        // k5 = dy/dt evaluated at time (t0+C61*h) for the state vector (Y0+(h*C62)*k0+(h*C63)*k1+(h*C64)*k2+(h*C65)*k3+(h*C66)*k4)
        state.setTime(t0+C61*h);	
        y_tmp = Y0+(h*C62)*k0+(h*C63)*k1+(h*C64)*k2+(h*C65)*k3+(h*C66)*k4;	
        state.setY(y_tmp.Ptr());
        MBS.calcSystemAcceleration(state);
        state.getYDot(k5.wPtr());

        // k6 = dy/dt evaluated at time (t0+C71*h) for the state vector Y1
        Y1 = Y0+(h*C72)*k0+(h*C73)*k1+(h*C74)*k2+(h*C75)*k3+(h*C76)*k4+(h*C77)*k5;
        state.setTime(t0+C71*h);		
        state.setY(Y1.Ptr());
        MBS.calcSystemAcceleration(state);
        state.getYDot(k6.wPtr());
        
        // sFlag
        this->estimateLocalStepError(   Y0,
                                        Y1,
                                        CE1*k0+CE2*k1+CE3*k2+CE4*k3+CE5*k4+CE6*k5+CE7*k6, 
                                        h,
                                        error);
        
}


void Ode45::interpolate(const double& t0, 
                        const Vector& y0, 
                        const Vector& y0Dot, 
                        const Vector& y1Dot, 
                        const double& h, 
                        const double& S1, 
                        Vector& Y){

        // See documentation for ntrp45.m of Matlab. This implementation is identical
        // to that of ntrp45.m
        const double B11 = 1.0;
        const double B12 = -183.0/64.0;
        const double B13 = 37.0/12.0;
        const double B14 = -145.0/128.0;

        const double B32 = 1500.0/371.0;
        const double B33 = -1000.0/159.0;
        const double B34 = 1000.0/371.0;

        const double B42 = -125.0/32.0;
        const double B43 = 125.0/12.0;
        const double B44 = -375.0/64.0;

        const double B52 = 9477.0/3392.0;
        const double B53 = -729.0/106.0;
        const double B54 = 25515.0/6784.0;

        const double B62 = -11.0/7.0;
        const double B63 = 11.0/3.0;
        const double B64 = -55.0/28.0;

        const double B72 = 3.0/2.0;
        const double B73 = -4.0;
        const double B74 = 5.0/2.0;

        const double S2 = S1*S1;
        const double S3 = S1*S2;
        const double S4 = S1*S3;
        
        const Vector& k0 = y0Dot;
        const Vector& k2 = k[1];
        const Vector& k3 = k[2];
        const Vector& k4 = k[3];
        const Vector& k5 = k[4];
        const Vector& k6 = y1Dot;
        
        //sFlag
        Y = y0 + (h*S1*B11)*k0;
        Y += (h*S2*B12)*k0 + (h*S2*B32)*k2 + (h*S2*B42)*k3 + (h*S2*B52)*k4 + (h*S2*B62)*k5 + (h*S2*B72)*k6;
        Y += (h*S3*B13)*k0 + (h*S3*B33)*k2 + (h*S3*B43)*k3 + (h*S3*B53)*k4 + (h*S3*B63)*k5 + (h*S3*B73)*k6;
        Y += (h*S4*B14)*k0 + (h*S4*B34)*k2 + (h*S4*B44)*k3 + (h*S4*B54)*k4 + (h*S4*B64)*k5 + (h*S4*B74)*k6;	
}

void Ode45::initIntegrator(const Vector& Y){
        // allocate space for k
        for(int i=0;i<5;k[i++].resize(numY_));
}

// @book{hairer1993,
//   title={Solving ordinary differential equations: Nonstiff problems},
//   author={Hairer, E. and N{\o}rsett, S.P. and Wanner, G.},
//   year={1993},
//   publisher={Springer}
// }